using System;
using System.Collections.Generic;
using System.Text;

namespace AI.AStar
{
    public class AStarPathfinder
    {
        #region Members

        static List<AStarNode> m_OpenList = new List<AStarNode>();
        static List<AStarNode> m_ClosedList = new List<AStarNode>();
        static List<IAStarNode> m_Path = new List<IAStarNode>();

        #endregion

        public static List<IAStarNode> FindShortestPath(IAStarNode start, IAStarNode target, IAStarSearchSpace search_space)
        {
            // reset all lists
            m_Path.Clear();
            m_OpenList.Clear();
            m_ClosedList.Clear();

            // add the start node in the open list
            AStarNode startNode = new AStarNode(start, null, 0, start.CalculateHCostTo(target));
            m_OpenList.Add(startNode);

            while (m_OpenList.Count > 0)
            {
                AStarNode currentNode = (AStarNode)PopNodeWithLowestF(m_OpenList);
                if (currentNode.NodeData == target)
                {
                    // we found the target node! now create the path
                    while (currentNode != null)
                    {
                        m_Path.Add(currentNode.NodeData);
                        currentNode = currentNode.Parent;
                    }
                    m_Path.Reverse();    //reverse the path to get it in the right order (from start to finish)
                    return m_Path;
                }

                // if the current node is not the target get its successors
                List<IAStarNode> successors = currentNode.NodeData.Successors(search_space);

                foreach (IAStarNode successor in successors)
                {
                    // Create the actual AStarNode (keeping track of parent and G and H costs)
                    AStarNode successorNode = new AStarNode(successor,
                        currentNode,
                        currentNode.G + currentNode.NodeData.CalculateGCostTo(successor),
                        successor.CalculateHCostTo(target));

                    // check to see if it's in the open list
                    AStarNode nodeOpen = null;
                    if(m_OpenList.Contains(successorNode))
                    {
                        nodeOpen = ((AStarNode)m_OpenList[m_OpenList.IndexOf(successorNode)]);
                        if (successorNode.F > nodeOpen.F)
                        {
                            // if the total F cost is greater than the one already in the open list
                            // it's no better so disregard it
                            continue;
                        }
                    }

                    // check to see if it's in the closed list
                    AStarNode nodeClosed = null;
                    if (m_ClosedList.Contains(successorNode))
                    {
                        nodeClosed = ((AStarNode)m_ClosedList[m_ClosedList.IndexOf(successorNode)]);
                        if (successorNode.F > nodeClosed.F)
                        {
                            // if the total F cost is greater than the one already in the closed list
                            // it's no better so disregard it
                            continue;
                        }
                    }

                    m_OpenList.Remove(nodeOpen);

                    m_ClosedList.Remove(nodeClosed);

                    m_OpenList.Add(successorNode);
                }

                m_ClosedList.Add(currentNode);
            }

            return m_Path;
        }

        private static AStarNode PopNodeWithLowestF(List<AStarNode> list)
        {
            int f = 0;
            AStarNode nodeWithLowestF = list[0];
            foreach (AStarNode node in list)
            {
                int local_f = node.F;
                if (local_f < f)
                {
                    nodeWithLowestF = node;
                    f = local_f;
                }
            }
            // pop it from the list
            list.Remove(nodeWithLowestF);
            return nodeWithLowestF;
        }
    }
}
